#pragma once

#include <ros/ros.h>
#include "unitree_legged_sdk/unitree_legged_sdk.h"
#include <unitree_legged_msgs/LowState.h>
#include "stair_ctrl/JoyStick.h"

#include <random>

#include "convert.h"


class simWirelessRemote
{
private:
    xRockerBtnDataStruct key_data;
    // stair_ctrl::JoyStick joy_stick;
    
    ros::Subscriber sim_state_sub;
    ros::Subscriber fake_js_sub;
    ros::Publisher low_state_pub;

    std::default_random_engine e;
    std::normal_distribution<float> n{0.0, 1.0};
    
    float noise_acc = 0.1;

public:
    simWirelessRemote(ros::NodeHandle *nh,
                    std::string sim_state_topic,
                    std::string fake_joy_stick_topic,
                    std::string low_state_topic)
    {
        auto hints = ros::TransportHints().tcpNoDelay();
        sim_state_sub = nh->subscribe(sim_state_topic, 100, &simWirelessRemote::simStateCallback, this, hints);
        fake_js_sub = nh->subscribe(fake_joy_stick_topic, 100, &simWirelessRemote::fakeJsCallback, this, hints);

        low_state_pub = nh->advertise<unitree_legged_msgs::LowState>(low_state_topic, 100);

        nh->param<float>("/sim_wireless_remote/noise_acc", noise_acc, 0.1);
    }

    void simStateCallback(unitree_legged_msgs::LowState::Ptr msg)
    {
        unitree_legged_msgs::LowState low_state = *msg.get();
        memcpy(&low_state.wirelessRemote[0], &key_data, 40);

        low_state.imu.accelerometer[0] += noise_acc * n(e);
        low_state.imu.accelerometer[1] += noise_acc * n(e);
        low_state.imu.accelerometer[2] += noise_acc * n(e);
        
        low_state_pub.publish(low_state);
    }

    void fakeJsCallback(const stair_ctrl::JoyStick::ConstPtr &msg)
    {
        key_data.head[0] = msg->head[0];
        key_data.head[1] = msg->head[1];

        key_data.lx = msg->lx;
        key_data.rx = msg->rx;
        key_data.ly = msg->ly;
        key_data.ry = msg->ry;
        key_data.L2 = msg->L2;

        for (int i = 0; i < 16; ++i)
        {
            key_data.idle[i] = msg->idle[i];
        }

        key_data.btn.components.R1 = msg->keys.R1;
        key_data.btn.components.L1 = msg->keys.L1;
        key_data.btn.components.start = msg->keys.start;
        key_data.btn.components.select = msg->keys.select;
        key_data.btn.components.R2 = msg->keys.R2;
        key_data.btn.components.L2 = msg->keys.L2;
        key_data.btn.components.F1 = msg->keys.F1;
        key_data.btn.components.F2 = msg->keys.F2;
        key_data.btn.components.A = msg->keys.A;
        key_data.btn.components.B = msg->keys.B;
        key_data.btn.components.X = msg->keys.X;
        key_data.btn.components.Y = msg->keys.Y;
        key_data.btn.components.up = msg->keys.up;
        key_data.btn.components.right = msg->keys.right;
        key_data.btn.components.down = msg->keys.down;
        key_data.btn.components.left = msg->keys.left;
    }

    void start()
    {
        ros::spin();
    }
};